#define _USE_MATH_DEFINES

#include <exception>
#include "Utils.h"
#include "Ellipsoid.h"

using namespace Core;

Ellipsoid* Ellipsoid::Wgs84 = new Ellipsoid(6378137.0, 6378137.0, 6356752.314245);
Ellipsoid* Ellipsoid::ScaledWgs84 = new Ellipsoid(1.0, 1.0, 6356752.314245 / 6378137.0);
Ellipsoid* Ellipsoid::UnitSphere = new Ellipsoid(1.0, 1.0, 1.0);

Vector3D Ellipsoid::CentricSurfaceNormal(const Vector3D &positionOnEllipsoid)
{
	return glm::normalize(positionOnEllipsoid);
}


Ellipsoid::Ellipsoid(double x, double y, double z) : Ellipsoid(Vector3D(x, y, z))
{
	
}

Ellipsoid::Ellipsoid(const Vector3D &radii)
{
	if ((radii.x <= 0.0) || (radii.y <= 0.0) || (radii.z <= 0.0))
	{
		throw std::exception("RADII");
	}

	_radii = radii;
	_radiiSquared = Vector3D(
		radii.x * radii.x,
		radii.y * radii.y,
		radii.z * radii.z);
	_radiiToTheFourth = Vector3D(
		_radiiSquared.x * _radiiSquared.x,
		_radiiSquared.y * _radiiSquared.y,
		_radiiSquared.z * _radiiSquared.z);
	_oneOverRadiiSquared = Vector3D(
		1.0 / (radii.x * radii.x),
		1.0 / (radii.y * radii.y),
		1.0 / (radii.z * radii.z));
}

Ellipsoid::~Ellipsoid()
{
}

Vector3D Ellipsoid::GeodeticSurfaceNormal(const Vector3D &positionOnEllipsoid) const
{
	return glm::normalize(Vector3D(
		_oneOverRadiiSquared.x * positionOnEllipsoid.x,
		_oneOverRadiiSquared.y * positionOnEllipsoid.y,
		_oneOverRadiiSquared.z * positionOnEllipsoid.z));
}

Vector3D Ellipsoid::GeodeticSurfaceNormal(const Geodetic3D &geodetic)const
{
	double cosLatitude = std::cos(geodetic.getLatitude());

	return Vector3D(
		cosLatitude * std::cos(geodetic.getLongitude()),
		cosLatitude * std::sin(geodetic.getLongitude()),
		std::sin(geodetic.getLatitude())
		);
}

std::vector<double> Ellipsoid::Inersections(const Vector3D &origin, const Vector3D &direction)
{
	std::vector<double> intersections;
	Vector3D normalizedDirection = glm::normalize(direction);

	// By laborious algebraic manipulation....
	double a =	direction.x * direction.x * _oneOverRadiiSquared.x +
				direction.y * direction.y * _oneOverRadiiSquared.y +
				direction.z * direction.z * _oneOverRadiiSquared.z;
	double b = 2.0 *
			(origin.x * direction.x * _oneOverRadiiSquared.x +
			 origin.y * direction.y * _oneOverRadiiSquared.y +
			 origin.z * direction.z * _oneOverRadiiSquared.z);
	double c =	origin.x * origin.x * _oneOverRadiiSquared.x +
				origin.y * origin.y * _oneOverRadiiSquared.y +
				origin.z * origin.z * _oneOverRadiiSquared.z - 1.0;

	// Solve the quadratic equation: ax^2 + bx + c = 0.
	// Algorithm is from Wikipedia's "Quadratic equation" topic, and Wikipedia credits
	// Numerical Recipes in C, section 5.6: "Quadratic and Cubic Equations"
	double discriminant = b * b - 4 * a * c;
	if (discriminant < 0.0)
	{
		// no intersections
		return intersections;
	}
	else if (discriminant == 0.0)
	{
		// one intersection at a tangent point
		intersections.push_back(-0.5 * b / a);
		return intersections;
	}

	double t = -0.5 * (b + (b > 0.0 ? 1.0 : -1.0) * std::sqrt(discriminant));
	double root1 = t / a;
	double root2 = c / t;

	// Two intersections - return the smallest first.
	if (root1 < root2)
	{
		intersections.push_back(root1);
		intersections.push_back(root2);
		return intersections;
	}
	else
	{

		intersections.push_back(root2);
		intersections.push_back(root1);
		return intersections;
	}
}

Vector3D Ellipsoid::ToVector3D(const Geodetic2D &geodetic) const
{
	return ToVector3D(Geodetic3D(geodetic.getLongitude(), geodetic.getLatitude(), 0.0));
}

Vector3D Ellipsoid::ToVector3D(const Geodetic3D &geodetic) const
{
	Vector3D n = GeodeticSurfaceNormal(geodetic);
	Vector3D k = Vector3D(
		_radiiSquared.x * n.x,
		_radiiSquared.y * n.y,
		_radiiSquared.z * n.z
		);

	double gamma = std::sqrt(
		(k.x * n.x) + 
		(k.y * n.y) +
		(k.z * n.z)
		);

	Vector3D rSurface = Vec3Divide(k, gamma);
	return rSurface + Vec3Multiply(n,geodetic.getHeight());
}

std::vector<Geodetic3D> Ellipsoid::ToGeodetic3D(std::vector<Vector3D>& positions) const
{
	std::vector<Geodetic3D> geodetics;

	std::vector<Vector3D>::iterator iter = positions.begin();
	for (; iter != positions.end(); iter++)
	{
		geodetics.push_back(ToGeodetic3D(*iter));
	}

	return geodetics;

}

Geodetic3D Ellipsoid::ToGeodetic3D(const Vector3D &position) const
{
	Vector3D p = ScaleToGeodeticSurface(position);
	Vector3D h = position - p;
	double height = glm::sign(glm::dot(h, position)) * glm::sqrt(glm::dot(h, h));

	return Geodetic3D(ToGeodetic2D(p), height);
}

std::vector<Geodetic2D> Ellipsoid::ToGeodetic2D(std::vector<Vector3D>& positions) const
{
	std::vector<Geodetic2D> geodetics;
	std::vector<Vector3D>::iterator iter = positions.begin();
	for (; iter != positions.end(); iter++)
	{
		geodetics.push_back(ToGeodetic2D(*iter));
	}
	return geodetics;
}

Geodetic2D Ellipsoid::ToGeodetic2D(const Vector3D &positionOnEllipsoid) const
{
	Vector3D n = GeodeticSurfaceNormal(positionOnEllipsoid);
	return Geodetic2D(
		std::atan2(n.y, n.x),
		std::asin(n.z / (glm::sqrt(glm::dot(n, n))))
		);
}

Vector3D Ellipsoid::ScaleToGeodeticSurface(const Vector3D &position) const
{
	double beta = 1.0 / std::sqrt(
		(position.x * position.x) * _oneOverRadiiSquared.x +
		(position.y * position.y) * _oneOverRadiiSquared.y +
		(position.z * position.z) * _oneOverRadiiSquared.z);
	double n = ComputeMagnitude(Vector3D(
		beta * position.x * _oneOverRadiiSquared.x,
		beta * position.y * _oneOverRadiiSquared.y,
		beta * position.z * _oneOverRadiiSquared.z));
	double alpha = (1.0 - beta) * (ComputeMagnitude(position)/ n);

	double x2 = position.x * position.x;
	double y2 = position.y * position.y;
	double z2 = position.z * position.z;

	double da = 0.0;
	double db = 0.0;
	double dc = 0.0;

	double s = 0.0;
	double dSdA = 1.0;

	do
	{
		alpha -= (s / dSdA);

		da = 1.0 + (alpha * _oneOverRadiiSquared.x);
		db = 1.0 + (alpha * _oneOverRadiiSquared.y);
		dc = 1.0 + (alpha * _oneOverRadiiSquared.z);

		double da2 = da * da;
		double db2 = db * db;
		double dc2 = dc * dc;

		double da3 = da * da2;
		double db3 = db * db2;
		double dc3 = dc * dc2;

		s = x2 / (_radiiSquared.x * da2) +
			y2 / (_radiiSquared.y * db2) +
			z2 / (_radiiSquared.z * dc2) - 1.0;

		dSdA = -2.0 *
			(x2 / (_radiiToTheFourth.x * da3) +
			y2 / (_radiiToTheFourth.y * db3) +
			z2 / (_radiiToTheFourth.z * dc3));
	} while (std::abs(s) > 1e-10);

	return Vector3D(
		position.x / da,
		position.y / db,
		position.z / dc);
}

Vector3D Ellipsoid::ScaleToGeocentricSurface(const Vector3D &position) const
{
	double beta = 1.0 / std::sqrt(
		(position.x * position.x) * _oneOverRadiiSquared.x +
		(position.y * position.y) * _oneOverRadiiSquared.y +
		(position.z * position.z) * _oneOverRadiiSquared.z);

	return Vec3Multiply(position,beta);
}

std::vector<Vector3D> Ellipsoid::ComputeCurve(const Vector3D &start, const Vector3D &stop, double granularity)const
{
	if (granularity <= 0.0)
	{
		throw std::exception("ComputeCurve()--Granularity must be greater than zero.");
	}

	Vector3D normal = glm::normalize(glm::cross(start, stop));
	double theta = AngleBetween(start,stop);
	int n = std::max((int)(theta / granularity) - 1, 0);

	std::vector<Vector3D> positions;

	positions.push_back(start);

	for (int i = 1; i <= n; ++i)
	{
		double phi = (i * granularity);

		positions.push_back(ScaleToGeocentricSurface(RotateAroundAxis(start,normal, phi)));
	}

	positions.push_back(stop);

	return positions;
}